%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%2345678901234567890123456789012345678901234567890123456789012345678901234567890
%        1         2         3         4         5         6         7         8

\documentclass[letterpaper, 10 pt]{article}


% The following packages can be found on http:\\www.ctan.org
%\usepackage{graphics} % for pdf, bitmapped graphics files
%\usepackage{epsfig} % for postscript graphics files
%\usepackage{mathptmx} % assumes new font selection scheme installed
%\usepackage{times} % assumes new font selection scheme installed
\usepackage{amsmath} % assumes amsmath package installed
%\usepackage{amssymb}  % assumes amsmath package installed
\usepackage[numbers,sort&compress]{natbib}
\usepackage{multicol}
\usepackage[bookmarks=true]{hyperref}
\usepackage{graphicx}        % standard LaTeX graphics tool
                             % when including figure files
\usepackage{amsfonts}
\usepackage{amsmath}
\usepackage{algorithm2e}
\usepackage{balance}  % to make the columns balanced on the last page
%\usepackage[margin=5pt]{subfig}
\usepackage[tight]{subfigure}
\usepackage[textsize=small, color=green]{todonotes}
%\usepackage{caption}
\usepackage{booktabs}

\newtheorem{theorem}{Theorem}


%\documentstyle[11pt]{article}
% Default margins are too wide all the way around. We reset them here
\setlength{\topmargin}{-.5in}
\setlength{\textheight}{9in}
\setlength{\oddsidemargin}{.125in}
\setlength{\textwidth}{6.25in}


\begin{document}
\title{Title}
\author{Authors\\
and Places}
\renewcommand{\today}{} % show no date
\maketitle



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\begin{abstract}

Abstract text.

\end{abstract}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{INTRODUCTION}
\label{sec: intro}

Many popular approaches to motion planning, especially for
high-dimensional systems, use the models of the environment and the
robot that are assumed to be static and known with
certainty. Unfortunately, in many realistic robotics scenarios, this
assumption is not always satisfied: the world model is rarely fully
known and is usually uncertain. Frequent and potentially significant
changes to it are caused either by sensor noise or by moving agents in
the environment. This motivates the need for efficient
\emph{replanning}: ``repairing'' a trajectory should it become
invalidated, in the spirit of the work by
\citet{stentz_hebert_ar95}. {CHOMP-R}, the presented motion replanner
based on covariant gradient descent, effectively fills that need.

The proposed approach was inspired by the work of
\citet{ratliff_etal_09} in applying covariant optimization to motion
planning. We propose several modifications to their approach
in order to apply this optimization technique to trajectory
replanning. We demonstrate that the proposed algorithmic improvements
lead to as much as an order of magnitude improvement in runtime, on
average, without losses in planning quality in terms of convergence,
obstacle avoidance or trajectory smoothness. In addition, we introduce
workspace pose constraint satisfaction in the context of covariant
optimization -- a useful feature for manipulating certain objects
(e.g. carrying a mug without spilling), dual-arm manipulation and
other advanced tasks.

Similar to most numerical optimization approaches, the method performs
``local'' modifications to the trajectory, namely within a certain
region around the original trajectory, while the size of the region is
related to the local minima of the cost function.  However, this
observation is in fact one of the reasons for preferring the approach
in replanning: this \emph{locality} reduces the disturbances to the
trajectory following controller when switching from the original to
the replanned trajectory. Reduced trajectory deviation is likely to
simplify trajectory following by the mechanism. Many standard planning
paradigms do not offer this locality feature.

\begin{figure}[t]
  \centering
  \includegraphics[width=3in]{fig/pole_exp1_small.jpg}
  \caption{\textbf{Trajectory replanning in manipulation}. Robot manipulators can
    utilize the approach presented here for monitoring and replanning
    arm trajectories. We provide simulated and real robot results
    using the DARPA ARM platform in the figure. The robot attempts to
    avoid an obstacle that is unexpectedly introduced in its
    workspace.  }
\label{fig: app examples}
\end{figure}

In summary, CHOMP-R is attractive for motion replanning for
high-dimensional systems because it is well-founded in optimization
theory and easy to implement, while exhibiting good convergence even
in standard motion planning setting. It is the first covariant
optimization technique that is fast enough to be effective as a motion
replanner.




\subsection{Motivation}
\label{sec: motivation}

%% Any robot system operating in a partially known or changing
%% environment must have a component that monitors the trajectory
%% validity with respect to the world model, attempts to modify the
%% trajectory should it become invalidated, or declares that the
%% trajectory cannot be fixed and must be aborted. 

The replanning capability, described above, is emerging as especially
useful as robot systems become increasingly more advanced, featuring
perception and localization systems that provide new information at
high update rates. Under-utilizing this information by a planner that
does not react fast enough puts the robot at a disadvantage.

%% This capability has been prominent in mobile robotics for many years
%% \cite{stentz_hebert_ar95}, however it achieved relatively little
%% popularity in the domain of high-dimensional motion planning, including
%% manipulation. Some of the reasons include the complexity and
%% computational demands of the problem due to the large, complex search
%% spaces in this setting. Many autonomous robots, especially mobile
%% manipulators, have limited computation capacity on-board, which
%% further aggravates the challenge.


Moreover, robot designs also become increasingly more complex,
featuring more than one manipulator arm and other moving parts,
operating in complex, cluttered environments. Classical motion
planning techniques, including sampling- and search-based ones,
require runtime that is overly extensive for high-dimensional
replanning.  In contrast, often trajectory repair may be accomplished
with a relatively small, local modification to the trajectory, and many
classical planners appear to be over-powered for this task. Instead,
we propose a method based on numerical optimization that offers
complexity advantages with respect to the conventional methods.

Lastly, by virtue of reasoning about the distance to obstacles in a
continuous fashion, numerical optimization approaches yield
\emph{maximum margin} quality to obstacle avoidance: not only is the
collision avoided, but the distance to the nearest obstacle is also
\emph{maximized}. This quality is useful in avoiding collision during
trajectory following even in the event that the robots models of the
environment and of its own motion are inaccurate.

In summary, using a re-planner that is too slow to react has a
disadvantage of causing the robot to operate more slowly than it would
otherwise be able to do. Trajectory locality, smoothness and maximing
obstacle avoidance margin are also attractive for increasing the speed
of the mechanism -- and both are naturally provided by the proposed
approach.


%% Making fast, simple geometric corrections to the
%% trajectory as a surrogate for replanning suffers from the velocity
%% discontinuities in the trajectory and similar artifacts
%% \cite{sekhavat_etal.ijrr98}, which would also slow down the execution
%% of the trajectory.

%% Lastly, the application of a numerical optimization approach
%% to replanning is motivated by the fact that it is likely to 
%% generate small modifications to the trajectory, namely those that 
%% belong to the same homotopy class as the original trajectory.
%% Small trajectory deviation is beneficial because it is likely
%% to simplify trajectory following by the mechanism. In
%% contrast, many other planning paradigms, including randomized ones,
%% do not offer such a similarity feature.



\subsection{Prior Work}

Motion replanning has been a popular method of managing changing and
uncertain environments in the domain of wheeled vehicle navigation for
several decades, and a number of approaches appear to be standard
components of fielded systems \cite{stentz_hebert_ar95,kelly_etal_04,
  Likhachev2009a}. However, the application of these methods to
general, high dimensional systems has been limited due to their
susceptibility to the ``curse of dimensionality'': in large
configuration spaces, the methods based on exhaustive search
suffer from intractability.

To overcome these issues, numerical optimization has been applied in
this domain. \citet{khatib.ijrr86} pioneered the application of the
artificial potential field for real-time obstacle avoidance. The
method's sensitivity to local minima has been addressed in a range of
related work. Analytical navigation functions that are free of local
minima have been proposed for some specific environments by
\citet{Rimon1988}. Numerical alternatives of a more general nature
were explored by \citet{barraquand_latombe.ijrr91}, however they
require an exponential amount of memory with respect to the dimension
of the configuration space. \citet{quinlan_khatib_icra93} further
extended numerical methods in motion replanning by modeling the
trajectory as a mass-spring system, an \emph{elastic band}, and
performing replanning by scanning back and forth along the elastic,
while moving one mass particle at a time. An extensive effort is made
to construct an explicit $\mathcal{C}$-space model of the free space,
and the cost function contains terms that attempts to control the
motion of the trajectory particles along the elastic.
\citet{brock_khatib_02} further extend this technique. Much of the
complexity of these methods is alleviated by the method proposed
herein via \emph{covariant} gradient descent, introduced by
\citet{ratliff_etal_09}. Obstacles are considered directly in the
robot's workspace, where the notions of distance and inner product are
more natural. While our method shares a number of design aspects with
\cite{ratliff_etal_09}, certain key components are structured
differently so as to apply covariant optimization to motion
replanning.  As described in the following sections, these
modifications span the core components of the algorithm, from the cost
function formulation to the world model representation.

%% an
%% optimization technique that is independent of the choice of trajectory
%% parameterization and, by virtue of spreading the effect of the
%% gradient steps across the trajectory, leads to maintaining trajectory
%% smoothness during the trajectory updates and to better convergence than
%% the standard gradient descent, without the need to model the interaction
%% of trajectory samples explicitly.

\citet{kalakrishnan_etal_icra11} addressed the motion planning problem
by applying motion sampling techniques.  However, the method ignores
gradient information that is, in fact, readily available and can be
quite useful to improve convergence.  In contrast, our approach
exploits the availability of the gradient. Our assumption that it
exists is frequently satisfied and does not hinder the application of
the method in a variety of relevant contexts, such as collision
avoidance and workspace constraint satisfaction, as we demonstrate in
the sequel.

Sampling-based \cite{Kavraki1996, Hsu1997, Kuffner2000} and
search-based \cite{cohen_chitta_likhachev_icra10} methods have been
applied to motion planning for high-dimensional systems, including
manipulators. workspace constraint satisfaction was also demonstrated
\cite{berenson_etal_icra09}. However, as suggested above, the
application of these methods to trajectory replanning has been
limited, since these methods are most effective at motion planning at
the ``global'' scale and do not attempt to leverage the proximity to
the initial trajectory being repaired. In contrast, the locality of
trajectory updates by the numerical optimization methods is
convenient, as motivated in Section \ref{sec: motivation}.

Much of {CHOMP-R}'s complexity advantages come from a pre-computed,
compositional representation of the environment. Boundary
representations of the environment have been previously developed
using geometric primitives by \citet{gilbert_etal_jra88,
  bobrow_ijrr89} and \citet{lin_canny_icra91}. Efficient hierarchical
representations have also been proposed by \citet{faverjon_icra89,
  quinlan_thesis} and others.  In contrast, we utilize the models of
the actual objects in the environment, which allows pre-computation of
very accurate, high-resolution distance field representations of the
objects.  The assumption that object shape information is known
\emph{a-priori} is based on the observation that these data are
becoming increasingly available for many common objects, facilitated
by the recent advances in RGB-D and similar sensing technology
\cite{Lai2011a}. In this regard, the proposed method seeks to explore
the symbiotic relationship between perception and planning by
leveraging the recent advances in the former to attain new gains in
the latter.





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% \section{TRAJECTORY REPLANNING VIA COVARIANT OPTIMIZATION}

%% The problem of trajectory replanning that was described in 
%% general terms in Section \ref{sec: intro} is formulated in 
%% this section, along with relevant nomenclature.

\subsection{Problem Formulation}

The trajectory replanning problem requires five parameters: $W_{\mathrm{obst}},
Q, J, u_r, \mathcal{F}$. We assume the robot \emph{workspace} to be $W
\subset SE(3)$, and $W_{\mathrm{obst}} \subset W$ is a subset that is occupied
by the obstacles or other regions that the robot must
avoid. Environment collisions, manipulator end-effector pose and similar
constraints are most naturally specified in the workspace. The subset of
the workspace that is free of obstacles is $W_{\mathrm{free}} = W \setminus
W_{\mathrm{obst}}$.  The robot \emph{configuration space} or
$\mathcal{C}$-space, $Q \subset \mathbb{R}^n$, is an $n$-dimensional
compact differentiable manifold, equipped with a metric $\rho$. The
Jacobian $J$ captures the relationship between $W$ and $Q$.

We define the \emph{control} space, $\mathcal{U}$, a Hilbert space
consisting of trajectories, or \emph{controls}, $u: \mathbb{R}
\rightarrow Q$. A common way to parameterize the controls is
w.r.t. time, however by virtue of covariant gradient that we will use,
this parameterization is not important here; instead, we assume the
domain of a control to be an interval $[0, 1] \in \mathbb{R}$. The
argument to $u$ will be omitted frequently for brevity.

The given \emph{reference trajectory}, $u_r$, is a control that has
been invalidated due to a change in the world model and must be
replanned. It encodes the boundary conditions for the replanning
problem: $q_{I} = u(0)$ and $q_{F} = u(1)$. Lastly, a given
time-independent \emph{cost function}
$\mathcal{F}:~\mathcal{U}~\rightarrow~\mathbb{R}$ assigns a scalar
cost to steering the system with a control $u \in \mathcal{U}$. In
practice, cost may be related to time duration of the associated
control, energy consumed by the system during the motion, risk
experienced, etc. We define the cost function as a weighted sum of
component functions, representing a number of the desired performance
criteria:

\begin{equation}
\mathcal{F}(u) = \sum_i^k w_i f_i(u)
\label{eq: cost sum}
\end{equation}

\noindent for $u \in \mathcal{U}$ and weight vector $\mathbf{w} \in
\mathbb{R}^k$. We also define a workspace \emph{path} of a point $b$,
an element of the robot's body $\mathcal{B}$, that is in motion while
the robot executes a trajectory $u$ as the projection of this
trajectory on the position subspace of $W$, $\pi_{b}: [0, 1]
\rightarrow \mathbb{R}^3$.

Then, the problem of replanning is computing a new
control, $u^*$, that minimizes the cost function, given problem
constraints:

\begin{equation}
  \begin{aligned}
    & u^* = & \underset{u \in \mathcal{U}}{\text{argmin}}  & \mathcal{F}(u) \\
    &       \text{s.t.}   &                      &  u(0) = u_r(0)      \\
    &                     &                      &  u(1) = u_r(1)      \\
    &                     &                      &  \pi_b(t) \in W_{\mathrm{free}}, \forall t \in \left[0, 1\right], \forall b \in \mathcal{B}
  \end{aligned}
  \label{eq: motion planning problem}
\end{equation}






\section{Compositional Distance Field}
\label{sec: object primitives}

In the numerical optimization context, obstacle avoidance is performed
by transforming a model of the environment into a continuous and
differentiable cost function. One approach to obtaining such a cost
function is to use a signed distance field $d: \mathbb{R}^3
\rightarrow \mathbb{R}$ which gives the distance from a point
$\mathbf{x} \in \mathbb{R}^3$ to the boundary of the nearest
obstacle. Values of $d(\cdot)$ are negative inside obstacles, positive
outside, and zero at the boundary.

A number of fast techniques for computing discrete distance fields
exist; for example, a method due to
\citet{felzenzwalb_huttenlocher_tr04} has linear complexity in the
number of voxels in the grid. Unfortunately, this computation still
takes several seconds on modern hardware for typical manipulator
reachability regions, even at the minimal admissible voxel resolution.
Moreover, it is beneficial to increase this resolution as much as
possible, as that yields a more informed representation and tends to
improve convergence of the optimizer.

However, many of the objects with which the robots nowadays must
interact cannot be easily represented as collections of such geometric
primitives.  The objects are almost never convex and typically have
complex shape, as illustrated in Figure \ref{fig: odp examples}.  In
addition, we note that the efficiency of many modern robot perception
systems is due to leveraging certain domain knowledge of the problem,
frequently involving detailed models of the known objects in
the environment.  In an effort to leverage a symbiotic relationship of
planning and perception, we propose utilizing the same object models
for efficient distance field computation.


%% Thus, on one hand,
%% recomputing the complete distance field of the whole environment at a
%% reasonably high resolution is prohibitively expensive, and on the
%% other, it is not clear how the obstacles and other objects of of
%% interest can be represented with closed-form geometric primitives.


\begin{figure}[t]
  \centering
  \subfigure[]{\includegraphics[width=0.7in]{fig/flashlight.png}}
  \subfigure[]{\includegraphics[width=0.7in]{fig/flashlight_edt.png}}
  \\
  \subfigure[]{\includegraphics[width=0.7in]{fig/drill_real.png}}
  \subfigure[]{\includegraphics[width=0.7in]{fig/drill_odp.png}}
  \caption{\textbf{Pre-computed object distance primitives}.  The
    figures shows two examples of objects with complex, non-convex
    shapes that would be difficult to represent accurately with simple
    geometric primitives. The corresponding object distance primitives
    are shown on the right in the figure.}
  \label{fig: odp examples}
\end{figure}



In particular, we note and exploit the fact that the environment
representations based on distance fields are compositional under the
\emph{min} operation. For every available object model, a
high-resolution distance field and its gradients are computed via an
extensive, but off-line computation in free space and with respect to
a certain frame of reference of the object, $F_O$. During planning, a
perception process generates a model of the environment which includes
a set $\mathcal{O}$ of objects and their poses, expressed with
homogeneous transforms $T_{F_O}^{F_W}$ in world frame $F_W$. Then the
distance field computation is reduced to a minimization across a set
of distance field primitives pre-computed for each object in
$\mathcal{O}$.


\begin{equation}
  d(\mathbf{x}) = \min_{O\in \mathcal{O}} \left( T_{F_O}^{F_W} \right)^{-1} \mathbf{x}
\end{equation}

Hierarchical representations, such as the $k$-d tree
\cite{bentley_75}, may be utilized to speed up this computation.





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Cost Functions}
\label{sec: cost functions}

Similar to most optimization methods, the basis of optimality of
trajectories computed by {CHOMP-R} is a cost function. We require that
this function be continuous and differentiable with respect to
trajectory parameterization in the relevant subset of the domain of
the function.


\subsection{Smoothness Cost}
\label{sec: smoothness cost}

Besides computing obstacle avoiding trajectories, we would like to
obtain trajectories that are smooth in terms of $\mathcal{C}$-space
dynamics, namely those with time derivatives of small magnitude. This
is beneficial for the accuracy of trajectory tracking and for
increasing the speed of execution.

To this end, we construct a \emph{smoothness} cost function,
$f_s(u)$ as a sum of squared $\mathcal{C}$-space derivatives of the
trajectory. For each derivative $d = 1, \ldots, D$, we assume a finite
differencing matrix $K_d$ and represent smoothness cost as a sum:

\begin{equation}
  f_s(u) = \frac{1}{2} \sum_{d=1}^D w_d \| K_d u + e_d \|^2  
\end{equation}

\noindent where constant $e_d$ encapsulates the boundary values $q_I$
and $q_F$, and $w_d$ is the weight of each of the cost components. 
The smoothness cost can be re-written as quadratic form:

\begin{equation}
\label{eq: quadratic form}
  f_s(u) = \frac{1}{2} u^T A u + u^T b + c
\end{equation}

\noindent for the appropriate constants, including a positive definite
$A$.  

This definition is similar to \citet{ratliff_etal_09}, however we
represent $u$ as an $n \times m$ matrix and avoid the Kronecker
(tensor) product, thereby reducing the complexity of the covariant
update rule (Section \ref{sec: covariant descent}) from $O(m^3n^3)$
down to $O(n)$, since we also replace the matrix multiply with a
tri-diagonal solve using the \emph{Thomas} algorithm
\cite{conte_deboor_72}.  This modification contributes to the reduction
in planner runtime reported in Figure \ref{fig: ros comparison}-E1 in
experimental results.



\subsection{Obstacle Cost}
\label{sec: obstacle cost}

The process of converting an environment model to a continuous and
differentiable function was given in Section \ref{sec: object
  primitives}. Here this function is used to formulate a cost function
that results in obstacle avoidance. We first review the design of the
obstacle cost function by \citet{ratliff_etal_09}. Then we propose an
alternative, \emph{minimax}, formulation of this function.

First, we define an obstacle potential function based on the distance
field:

\begin{equation}
  c \left( \mathbf{x} \right) =  \begin{cases}
    -d(\mathbf{x}) + \frac{1}{2}\epsilon, & \text{if $d(\mathbf{x}) < 0$} \\
    \frac{1}{2\epsilon} (d(\mathbf{x}) - \epsilon)^2  , & \text{if $0 \leq d(\mathbf{x}) \leq \epsilon$} \\
    0 & \text{otherwise}
  \end{cases} 
  \label{eq: obstacle potential}
\end{equation}

Integration of the obstacle potential over the entire robot, $\forall
b \in \mathcal{B}$, provides an estimate of the collision cost at a
given configuration.

\subsubsection{Integral Formulation}

\citet{ratliff_etal_09} estimate the collision cost of the entire
trajectory via a line integral along the trajectory:

\begin{equation}
  f_o(u) = \int_0^1 \int_{\mathcal{B}} c \left( \pi_b(t) \right) \| \frac{d}{dt} \pi_b (t) \| db dt
  \label{eq: obstacle cost}
\end{equation}

Because (\ref{eq: obstacle cost}) does not depend on higher order
derivatives beyond velocity, we can write functional gradient as
$\bar{\nabla} f_o = \frac{\partial v}{\partial q} - \frac{d}{dt}
\frac{\partial v}{\partial q'}$, where $v$ denotes everything inside
the time integral \cite{courant_hilbert_53,
  quinlan_thesis}. Application of this formula to (\ref{eq: obstacle
  cost}) yields:

\begin{equation}
  \bar{\nabla} f_o = \int_{\mathcal{B}} J^T \|\pi_b'\| \left( P \nabla c - c \kappa \right) du
\end{equation}

\noindent where $P = I - \hat{\pi_b}'\hat{\pi_b}'^T$ is a projection
matrix, $\kappa = \frac{1}{\|\pi_b'\|^2} P \pi_b''$ is the curvature
vector \cite{docarmo_76}, normalized vectors are denoted with
$\hat{\pi_b}$, prime notation denotes time derivatives, and time
dependence of $J$, $\pi_b$ and $c$ is suppressed for brevity.

%% In order to detect potential collision of the robot body,
%% $\mathcal{B}$, with the environment, we develop a sampling rule to
%% reduce the continuum of $\mathcal{B}$ into a finite set of samples.
%% One efficient way to accomplish that is to represent the robot as a
%% collection of overlapping spheres and capsules, or line-segment swept
%% spheres. For a sphere of radius $r$ with center $\mathbf{x}$, the
%% distance from any point in the sphere to the nearest obstacle is no
%% less than $d(\mathbf{x}) − r$. A lower bound for capsules may be
%% computed similarly.


\subsubsection{Minimax Formulation}

%% Even though integral formulation in the previous section is a
%% straight-forward one in terms of continuous formulation, most
%% representations on computing hardware are discrete. The integral can
%% be formulated as a Riemann sum, however, discretization effects may
%% cause difficulties in especially cluttered environments, especially
%% the human-centric ones that contain many thin obstacle structures,
%% such as walls and doors. If the reference trajectory is in collision,
%% it becomes possible for robot body samples to appear on the opposite
%% sides of the thin obstacle; collision potential (\ref{eq: obstacle
%%   potential}) will act on these body samples in different directions,
%% thereby confusing the algorithm as to the desired direction to move
%% the arm in order to bring it out of collision.

On certain systems, such as low-power embedded controllers, floating
point matrix arithmetic is an expensive operation.  In such
applications, we note potential value of redefining the obstacle
cost as a minimax formulation allows minimizing this computation,
while still attaining much of {CHOMP-R} performance. In particular, we
replace the line integral in (\ref{eq: obstacle cost}) with a
\emph{max} operator:

\begin{equation}
  f_o(u) = \max_i \left( \int_{\mathcal{B}} c \left( \pi_u(u(t)) \right) \| \frac{d}{dt} \pi_u (u(t)) \| du \right)
  \label{eq: minimax obstacle cost}
\end{equation}

\noindent for trajectory samples $i \in \{0, \ldots, 1\}$. As a
result, the cost function will seek to minimize the worst violation of
the obstacle constraint and will not be influenced by other trajectory
points. Once the maximum value of cost is alleviated, the planner will
shift focus to bring another body element out of collision, eventually
leading to obstacle avoidance by the entire trajectory. While
(\ref{eq: obstacle cost}) repeats the computation of the body integral
over a large number of trajectory samples, this formulation performs
it only once and is, therefore, less demanding computationally. 

This formulation, however, is likely to require a greater number of
optimization iterations, since each iteration is somewhat less
informed than the integral formulation where all trajectory points are
used.  This trade-off may be managed by utilizing several high-cost
trajectories instead of the single maximizing one in (\ref{eq: minimax
  obstacle cost}).  The choice of this trade-off largely depends on
the specifics of the computing platform at hand and the runtime cost
of matrix arithmetic on it.



\subsection{Workspace Cost}

In many settings, it is beneficial to impose a cost on certain
workspace motion of the mechanism. For example, in the context of
manipulation, visual servoing, etc. one might desire to constrain the
end-effector of the manipulator to be in a certain, maybe small,
subset of the workspace.

Given a certain element of the robot body, $b \in \mathcal{B}$ and its
workspace position $(x, y, z)$ and orientation $(\phi, \gamma,
\theta)$, we propose setting up a per-dimension cost function that
evaluates to $0$ if $b$ satisfies the desired constraint and to
quadratic error if it does not. Using the $x$- dimension as an
example, and supposing we desire to limit $b$'s pose such that its
$x$-coordinate is greater than $x_{\mathrm{min}}$, we can define a
workspace cost potential similar to (\ref{eq: obstacle potential}):

\begin{equation}
%  a = b
  c \left( \mathbf{x} \right) =  \begin{cases}
    (x - x_{\mathrm{min}})^2 & \text{if $x < x_{\mathrm{min}}$} \\
    0 & \text{otherwise}
  \end{cases} 
  \label{eq: workspace cost}
\end{equation}

If the desired interval is closed, another instance of (\ref{eq:
  workspace cost}) can be used for the other boundary,
e.g. $x_{\mathrm{max}}$. This potential can be used in a manner
similar to Section \ref{sec: obstacle cost}, e.g. from (\ref{eq:
  obstacle cost}) onwards.  This cost function is well-defined for any
$b \in \mathcal{B}$, and is continuous and differentiable.






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{TRAJECTORY REPLANNING VIA COVARIANT OPTIMIZATION}

This Section combines the components presented previously: the
compositional distance fields in Section \ref{sec: object primitives}
and the cost functions in Section \ref{sec: cost functions}, and
applies them to motion replanning in the covariant optimization
framework.


\subsection{Covariant Gradient Descent}
\label{sec: covariant descent}

We obtain a trajectory update rule by computing a gradient of the cost
function with respect to trajectory parameterization. A first-order
Taylor expansion of the cost function during an update step is:

\begin{equation}
  \mathcal{F}(u) \approx \mathcal{F}(u_{k}) + \nabla \mathcal{F}(u_k)^T (\Delta u)
  \label{eq: taylor exp}
\end{equation}

\noindent where $\Delta u = u - u_k$. This allows us to write the update similar to \cite{ratliff_etal_09}:

\begin{equation}
  u_{k+1} = \underset{u \in \mathcal{U}}{\text{argmin}} \left( \mathcal{F}(u_k) + \nabla \mathcal{F}(u_k)^T \Delta u + \frac{\lambda}{2} \| \Delta u \|^2_M \right)
  \label{eq: update rule}
\end{equation}

\noindent where $\| \cdot \|^2_M$ is the norm with respect to the
Riemannian metric $M$.  Setting the gradient of the right hand side of
(\ref{eq: update rule}) to zero and solving for the minimizer leads to:

\begin{equation}
  u_{k+1} = u_k - \frac{1}{\lambda} M^{-1} \nabla \mathcal{F}(u_k)
  \label{eq: smooth update}
\end{equation}

This update rule ensures that the change in the trajectory is small
with respect to the metric $M$. As given in
\citet{boyd_vandenberghe_04}, solving a regularized problem of the
form in (\ref{eq: update rule}) is equivalent to minimizing the linear
approximation in (\ref{eq: taylor exp}) within a ball around $u_k$
whose radius is related to the regularization constant $\lambda$. At
each trajectory update, only those modifications that keep the
trajectory smooth are considered.  We use $A$ in (\ref{eq: quadratic
  form}) as the metric, and therefore the action of the $M^{-1}$
operator in (\ref{eq: smooth update}) is, in basic terms, to spread
the gradient across the trajectory in order to maintain smoothness.



\subsection{Weight Scheduling}
\label{sec: weight scheduling}

\citet{ratliff_etal_09} and \citet{quinlan_thesis} use the weight
vector $\mathbf{w}$ in (\ref{eq: cost sum}) to specify the relative
importance of the components of the overall cost function being
minimized. However, we found value in modifying these weights during
optimization via a process referred to as \emph{weight scheduling},
similar to gain scheduling in non-linear control \cite{khalil_01}.
Early in optimization, it is beneficial to give most of the weight to
collision avoidance and similar costs and nearly remove the
influence of the smoothness cost. As optimization progresses and
trajectory obstacle cost significantly reduces, the weight is shifted to the
smoothness cost to ascertain that the trajectory remains attractive
with respect to dynamics measures. Note that, even while the optimizer
focuses on obstacle avoidance, the covariant trajectory updates still
maintain smoothness, as discussed in Section \ref{sec: covariant
  descent}.

This measure makes the overall cost function more robust to local
minima.  For example, if the reference trajectory $u_r$ being repaired
(due to a collision along it) is already smooth, then the action of
the collision cost will be to modify the trajectory. If this
modification would cause the trajectory to be less smooth, it will
force the smoothness cost to restore smoothness and bring the
trajectory back into collision. Indeed, the optimization may be at a
local minimum of this sort right at the outset. Experimental results
in Figure \ref{fig: ros comparison}-E3 suggest that weight scheduling
indeed improves performance in practice -- both in terms of better
planner convergence and the reduced number of required iterations.









%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Experimental Results}

Simulated and real-robot experimental results are presented in the
context of manipulation planning and demonstrate the efficacy of the
approach. This presentation is divided into two parts. First, we show
that, while the proposed solution is best suited for replanning, it
still outperforms the CHOMP algorithm \cite{ratliff_etal_09} in
general motion planning setting. Second, we apply the approach to
replanning on a physical robot and demonstrate fast, nearly 10Hz,
replan rates in realistic scenarios.

In comparing to the standard CHOMP, we utilized a freely available
implementation based on ROS \cite{ros_chomp_website}. A simulated
manipulator robot operates in a set of scenes that include dense
clutter due to a collection of 50 randomly placed box obstacles of
random size, as illustrated in Figure \ref{fig: clutter scene}. Ten different scenes
were generated and 100 planning queries were performed by selecting
random initial and final arm configurations.

\begin{figure}[t]
  \centering
  \includegraphics[width=2in]{fig/clutter_test_0.png}
  \caption{\textbf{Simulated clutter scene}. Comparison with CHOMP
    planner \cite{ratliff_etal_09} was performed in randomized
    simulated clutter scenes similar to this one. }
\label{fig: clutter scene}
\end{figure}

The ROS implementation was executed on these planning queries, and the
results were considered the control group, i.e. \emph{baseline}. The
methods described in this paper were then attempted also, and the
results were compared to this control group. Four different
configurations of CHOMP-R were compared to CHOMP:

\begin{itemize}
\item $E1$: CHOMP algorithm, implemented verbatim to
  \cite{ratliff_etal_09}. This experiment attempts to calibrate our
  implementation of the algorithm to ROS \cite{ros_chomp_website}.
\item $E2$: Same implementation as above, but the standard signed
  distance field was replaced with the compositional distance field in
  Section \ref{sec: object primitives}.
\item $E3$: Same implementation as $E1$, but cost function
  weights were scheduled as described in Section \ref{sec: weight
    scheduling}.
\item $E4$: {CHOMP-R}: both object distance primitives and weight
  scheduling (as in $E2$ and $E3$) were combined.
\end{itemize}

Figure \ref{fig: ros comparison} demonstrates how the four variants of
the algorithm compare to ROS CHOMP \cite{ros_chomp_website}. It uses
three measures for the comparison: the ratio of the planning queries
that were solved by a planner (failure means exceeding 500
iterations), the number of iterations a planner took and the amount of
time (in seconds) that was required to find a solution. These measures
are color-coded in the Figure as black, gray and white, respectively,
and presented as ratios: each of the quantities was measured with each
of the variants $E1$-$4$ and divided by the respective quantity
measured using ROS CHOMP.

In particular, the three bars corresponding to $E1$ demonstrate that
our implementation of standard CHOMP was able to solve a similar
number of motion planning queries in the simulated clutter environment
(the black bar is marginally above the 1.0 ratio mark), but required a
slightly greater number of iterations, on average across all
trials. The implementation exhibited somewhat lower runtime in
seconds, however, as Section \ref{sec: smoothness cost} attempted to
explain.

The experiment $E2$ suggests that replacing the standard distance
field with the compositional one does not affect performance, except by
requiring far less runtime (in seconds) for each planning query.  A
reduction of over an order of magnitude is attained by virtue of
eliminating the need to compute the distance field on-line during
planning.

The advantage of the cost function weight scheduling is demonstrated
in the experiment $E3$. The improvement is two-fold: both in the
greater ratio of queries solved and in reducing the number of
iterations -- with an advantage of about 20\% in each
category. Runtime in seconds is also lower as a consequence of the
reduction in iterations.



\begin{figure}
  \centering
  \includegraphics[width=3.2in]{fig/ros_comparison.pdf}
  \caption{\textbf{Planning performance comparison}. The algorithm
    proposed in this paper is evaluated in standard manipulator motion
    planning setting. Several aspects of the algorithm are compared to
    the CHOMP algorithm implementation in ROS
    \cite{ros_chomp_website}.}
\label{fig: ros comparison}
\end{figure}


Lastly, $E4$ compares {CHOMP-R}, the combination of the the features
exhibited in $E2$ and $E3$, to ROS CHOMP.  This instance of the design
is the one that we apply to the real robot replanning experiments that
follow.

The ARM-S manipulator robot, featuring a 7-DOF Barrett arm, was used
for physical experimentation. The arm repeatedly executes a
pre-defined trajectory, which becomes invalidated due to an unexpected
obstacle. Once the perception system localizes the obstacle, the
replanner reacts to it by modifying the trajectory to avoid the
obstacle.  100 trials of the experiment were performed in two
configurations: {CHOMP-R} with and without the end-effector workspace
pose constraint, compared to standard CHOMP. As Figure \ref{fig: real
  robot exp} shows, {CHOMP-R} resulted in notable replanning speedup
on this platform.


\begin{figure}
  \centering
  \includegraphics[width=3.2in]{fig/real_robot_exp_no_ee.pdf}
  \caption{\textbf{Physical robot experiments}. CHOMP and {CHOMP-R} are
    compared on the ARM-S robot platform. The figure shows that the
    constraint claimed greater runtime in seconds. The average CHOMP
    replan runtime was 2.03 sec, while the average {CHOMP-R} runtime was
    0.28 and 0.17 sec. with and without the end-effector (e.e.)
    constraint, respectively. This result is significant in
    demonstrating that {CHOMP-R} is sufficiently fast to be responsible
    for the robot's reactive behavior. In addition, it demonstrates
    that satisfying workspace pose constraint maintains sub-second
    replan rate. }
\label{fig: real robot exp}
\end{figure}




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{CONCLUSIONS}

We presented an approach to trajectory modification motivated by the
inevitable and frequent changes in the world model of autonomous
robots operating in partially-known, cluttered environments. The
motion replanner is well suited for general, high-dimensional systems,
including manipulators. The method is based on covariant gradient
descent which leads to significantly faster convergence in this
context than standard descent methods. The resulting replanner is
effective at modifying planned trajectories should they become in
collision due to changes in the environment model.

Future work includes efforts to increase search space exploration
capacity of the replanner by applying it in a parallel computation
setting by providing a range of spatially distinct initial
trajectories. By leveraging parallel computing hardware, a large
number of such parallel processes can be executed, resulting in
exploration capacity that, in this context, may potentially rival the
standard, global motion planning approaches.




%\addtolength{\textheight}{-12cm}   % This command serves to balance the column lengths
                                  % on the last page of the document manually. It shortens
                                  % the textheight of the last page by a suitable amount.
                                  % This command does not take effect until the next page
                                  % so it should come on the page before the last. Make
                                  % sure that you do not shorten the textheight too much.




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section*{ACKNOWLEDGMENT}

The authors gratefully acknowledge the contribution of the DARPA
Autonomous Robotic Manipulation Program. Special thanks to Gil Jones
and others at Willow Garage for their support in performing ROS
experiments.



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\bibliographystyle{plainnat}
\bibliography{biblio}




\end{document}
